import logging
import time
from Motor_ctl import Motor_CTL

class Robotchassis:
    """底盘电机控制封装"""
    def __init__(self, can_channel_handle, send_id=0x601, response_id=0x581, logger=None):
        self.can = can_channel_handle
        self.logger = logger or logging.getLogger(__name__)
        # 复用原来的 Motor_CTL 初始化方式
        self.motor_ctl = Motor_CTL(self.can, send_id, response_id)

    def setup_velocity_pdo(self):
        """设置速度模式PDO配置（与原控制器实现一致）"""
        self.logger.info("开始配置底盘速度模式PDO...")
        # 1. 设置控制模式
        self.motor_ctl.set_control_mode(self.motor_ctl.SYNC_CONTROL)
        # 2. 初始化电机
        if not self.motor_ctl.initialize_motor():
            self.logger.error("底盘速度模式初始化失败")
            return False
        # 3. 配置RPDO1用于接收目标速度指令（单电机映射，与test_pdo.py一致）
        mapped_objs_rpdo1 = [(self.motor_ctl.OD_TARGET_VELOCITY, 0x03, 4)]
        if not self.motor_ctl.init_pdo('rpdo1', mapped_objs_rpdo1, self.motor_ctl.PDO_TRANSMIT_EVENT):
            self.logger.error("底盘RPDO1初始化失败")
            return False
        # 4. 配置TPDO1用于反馈实际速度（单电机映射，与test_pdo.py一致）
        mapped_objs_tpdo1 = [(self.motor_ctl.OD_VELOCITY_ACTUAL_VALUE, 0x03, 4)]
        if not self.motor_ctl.init_pdo('tpdo1', mapped_objs_tpdo1, self.motor_ctl.PDO_TRANSMIT_EVENT):
            self.logger.error("底盘TPDO1初始化失败")
            return False
        self.logger.info("底盘速度模式PDO配置完成")
        return True

    def send_velocity_command(self, left_speed, right_speed):
        """发送底盘速度指令（带重试机制），与原控制器一致"""
        # 限幅到 [-20, 20]
        left_speed = max(min(left_speed, 20), -20)
        right_speed = max(min(right_speed, 20), -20)
        # 整数化
        left_speed_int = int(left_speed)
        right_speed_int = int(right_speed)
        left_bytes = left_speed_int.to_bytes(2, byteorder='little', signed=True)
        right_bytes = right_speed_int.to_bytes(2, byteorder='little', signed=True)
        pdo_data = list(left_bytes) + list(right_bytes)
        self.logger.info(f"底盘指令 - 左轮: {left_speed:.1f} r/min, 右轮: {right_speed:.1f} r/min")
        # 重试发送
        max_retries = 5
        retry_count = 0
        while retry_count < max_retries:
            if self.motor_ctl.send_pdo('rpdo1', pdo_data):
                time.sleep(0.1)
                self.logger.info("底盘速度指令发送成功")
                return True
            retry_count += 1
            self.logger.warning(f"底盘PDO发送失败，第{retry_count}次重试...")
            time.sleep(0.2)
        self.logger.error("底盘PDO发送失败，达到最大重试次数")
        return False

    def read_real_speed(self, timeout=0.05):
        """读取实际速度反馈（合并原start_velocity_parsing和_velocity_parsing_thread功能）
        
        Args:
            timeout: 接收超时时间（秒）
            
        Returns:
            tuple: (success, left_speed, right_speed) 
                   success: 是否成功读取
                   left_speed: 左轮速度（r/min）
                   right_speed: 右轮速度（r/min）
        """
        try:
            # 读取速度反馈数据
            success, data = self.motor_ctl.receive_standard_frame(0x281, timeout)
            
            if success and data and len(data) >= 4:
                # 正确解析速度数据：4字节数据包含左右轮速度各2字节
                # 数据格式：左轮速度（2字节小端） + 右轮速度（2字节小端）
                left_velocity_raw = int.from_bytes(bytes(data[0:2]), 'little', signed=True)
                right_velocity_raw = int.from_bytes(bytes(data[2:4]), 'little', signed=True)
                
                # 转换为r/min（根据驱动器规格调整系数）
                left_velocity = left_velocity_raw * 0.1
                right_velocity = right_velocity_raw * 0.1
                
                # 记录解析结果到日志
                self.logger.info(f"速度反馈 - 左轮: {left_velocity:.1f} r/min, 右轮: {right_velocity:.1f} r/min | "
                               f"原始数据: {[hex(d) for d in data]}")
                
                return True, left_velocity, right_velocity
            else:
                return False, 0.0, 0.0
                
        except Exception as e:
            self.logger.error(f"读取速度反馈错误: {e}")
            return False, 0.0, 0.0

    def Chassiscontroller(self):
        """底盘控制器示例，提供四个方向的运动控制"""        
        def forward(speed):
            """前进：左轮正转，右轮反转"""
            return self.send_velocity_command(speed, -speed)
        
        def backward(speed):
            """后退：左轮反转，右轮正转"""
            return self.send_velocity_command(-speed, speed)
        
        def right(speed):
            """原地右转：左轮正转，右轮正转"""
            return self.send_velocity_command(speed, speed)
        
        def left(speed):    
            """原地左转：左轮反转，右轮反转"""
            return self.send_velocity_command(-speed, -speed)
        def stop():
            """停止：左轮0速，右轮0速"""
            return self.send_velocity_command(0.0, 0.0)
        # 返回控制函数字典，方便调用
        return {
            'forward': forward,
            'backward': backward,
            'right': right,
            'left': left,
            'stop': stop
        }